
#include <stdio.h>
#include "member_BFixedRestraint.h"
#include "member_BRestraint.h"
#include "member_BBody.h"
#include "member_BNode.h"

using namespace BWE;

member_BFixedRestraint::member_BFixedRestraint(BFixedRestraint* restraint)
{
	boss = restraint;
}
member_BFixedRestraint::~member_BFixedRestraint()
{

}

void member_BFixedRestraint::calculateTransforms()
{
	const BBody* bodyA = restraint_member(boss)->bodyA;
	const BBody* bodyB = restraint_member(boss)->bodyB;

	const BMatrix& matrixA = node_member(bodyA)->matrix;
	const BMatrix& matrixB = node_member(bodyB)->matrix;

	BVector pointA = matrixA * restraint_member(boss)->pivotA;
	BVector pointB = matrixB * restraint_member(boss)->pivotB;
	BVector offset = pointB - pointA;
	linearPos = matrixA.inverse().mult3(offset);
	for (int i = 0; i < 3; i++)
	{
		FixedLimit& limit = linearLimits[i];
		BReal diff = linearPos[i];
		if (diff < 0)
		{
			limit.limited = true;
			limit.limitError = diff;
			continue;
		}
		if (diff > 0)
		{
			limit.limited = true;
			limit.limitError = diff;
			continue;
		}
		limit.limited = false;
		limit.limitError = 0;
	}

	calculatedAxis[0] = matrixA.row(0);
	calculatedAxis[1] = matrixA.row(1);
	calculatedAxis[2] = matrixA.row(2);

	BMatrix matrix = matrixB * matrixA.inverse();

	angularPos = matrix.euler();

	for (int i = 0; i < 3; i++)
	{
		RotLimit& limit = angularLimits[i];
		BReal diff = rotation[i] - angularPos[i];
		if (diff < 0)
		{
			limit.limited = true;
			limit.limitError = diff;
			continue;
		}
		if (diff > 0)
		{
			limit.limited = true;
			limit.limitError = diff;
			continue;
		}
		limit.limited = false;
		limit.limitError = 0;
	}
}
